#ifndef MINIMUM_SNAP_PATH_PLAN
#define MINIMUM_SNAP_PATH_PLAN


#include <ros/ros.h>
#include <iostream>
#include <geometry_msgs/PoseStamped.h>
#include <Eigen/Eigen>
#include <math.h>
#include "matplotlibcpp.h"
#include <numeric>
#include <fstream>
#include <sstream>


using namespace std;
using namespace Eigen;
namespace plt = matplotlibcpp;

///closed-form minimum_snap solution
// Example
    // Eigen::MatrixXd posList=MatrixXd::Zero(8,3);
    // posList<<0,0,0,
    //     -1,2.5,0,
    //     -2.5,5.2,0,
    //     -1.5,7.2,0,
    //     2.5,9.5,0,
    //     2,13.5,0,
    //     -2.8,18,0,
    //     0,22,0;
	// std::vector<double> start_end_va = {0,0,0,0,0,0,0,0};
	// double max_vel_=1.5;
	// double max_acc_=1.2;
	// Eigen::MatrixXd minimum_snap_pathList=Minimum_X_Plan(posList,30,start_end_va,max_vel_*0.6,max_acc_*0.9,1);


std::vector<double> getMinimum_X_PlanTrajTotalPointNum(MatrixXd point,double rate,std::vector<double> start_end_va ,
double set_speed,double set_acc, int option, int& traj_total_point_num );






std::vector<Eigen::Vector3d> Minimum_X_Plan(MatrixXd point,double rate,std::vector<double> start_end_va ,double set_speed,double set_acc,int option,
											int setTotalTrajPointNum);


std::vector<Eigen::Vector3d> Navie_Plan(MatrixXd point,double rate,std::vector<double> start_end_va ,double set_speed,double set_acc,int option,
										int setTotalTrajPointNum);
#endif
